#include "Grid.h"
#include "Renderable.h"
#include "Memory.h"
#include "Camera.h"

//helper funcs
Vec3 Grid::Calculate_Min()
{
    BBox box;
    Vec3 p0(FLT_MAX,FLT_MAX,FLT_MAX);

    vRenderables& vR = m_renderables;
    int numObjs = vR.size();
    for(int i = 0; i < numObjs; ++i)
    {
        if(!vR[i]->HasBox())
            vR[i]->CalculateBox();
        box = vR[i]->GetBox();
        if(box.m_minX < p0.m_x)
            p0.m_x = box.m_minX;
        if(box.m_minY < p0.m_y)
            p0.m_y = box.m_minY;
        if(box.m_minZ < p0.m_z)
            p0.m_z = box.m_minZ;
    }

    p0.m_x -= 0.001f;
    p0.m_y -= 0.001f;
    p0.m_z -= 0.001f;

    return p0;
}
Vec3 Grid::Calculate_Max()
{
    BBox box;
    Vec3 p0(FLT_MIN,FLT_MIN,FLT_MIN);

    vRenderables& vR = m_renderables;
    int numObjs = vR.size();
    for(int i = 0; i < numObjs; ++i)
    {
        if(!vR[i]->HasBox())
            vR[i]->CalculateBox();
        box = vR[i]->GetBox();
        if(box.m_maxX > p0.m_x)
            p0.m_x = box.m_maxX;
        if(box.m_maxY > p0.m_y)
            p0.m_y = box.m_maxY;
        if(box.m_maxZ > p0.m_z)
            p0.m_z = box.m_maxZ;
    }

    p0.m_x += 0.001f;
    p0.m_y += 0.001f;
    p0.m_z += 0.001f;

    return p0;
}
//main usage
void Grid::SetUpCells(const int x, const int y, const int z)
{
    //calculate the worlds box
    Vec3 min = Calculate_Min();
    Vec3 max = Calculate_Max();
    m_box.SetValues(min.m_x,min.m_y,min.m_z,max.m_x,max.m_y,max.m_z);

    //store these up here
    vRenderables& objects = m_renderables;
    int numObjs = objects.size();

    //calculate how many cells in each direction...if not passed in as a requested override
    //the size of the overall box
    float wx = max.m_x - min.m_x;
    float wy = max.m_y - min.m_y;
    float wz = max.m_z - min.m_z;

    //multiplier
    float mul = 2.0f;
    // volume divided by 'n', to the power of a value
    float s = pow(wx*wy*wz / (float)numObjs, 0.33333333f);
    m_x = static_cast<int>(mul * wx / s + 1) +1;
    m_y = static_cast<int>(mul * wy / s + 1) +1;
    m_z = static_cast<int>(mul * wz / s + 1) +1;

    if(m_x < x)
        m_x = x;
    if(m_y < y)
        m_y = y;
    if(m_z < z)
        m_z = z;

    //now create the memory and construct some pointers in the space
    int numCells = m_x * m_y * m_z;
    m_cells.reserve(numCells);

    for(int i = 0; i < numCells; ++i)
        m_cells.push_back(NULL);

    std::vector<int> counts;
    counts.reserve(numCells);

    for(int i = 0; i < numCells; ++i)
        counts.push_back(0);

    //put all the objects into the cells!

    BBox obj_box;       //the current objects box,
    int index;          //the cells array index

    //for each object
    for(int i = 0; i < numObjs; ++i)
    {
        obj_box = objects[i]->GetBox();

        //make sure the min and maximum of the box correspond to the correct grid cell index
        int ixMin = static_cast<int>(Clamp((obj_box.m_minX - min.m_x) * (float)m_x / (max.m_x - min.m_x), 0, (float)m_x - 1));
        int iyMin = static_cast<int>(Clamp((obj_box.m_minY - min.m_y) * (float)m_y / (max.m_y - min.m_y), 0, (float)m_y - 1));
        int izMin = static_cast<int>(Clamp((obj_box.m_minZ - min.m_z) * (float)m_z / (max.m_z - min.m_z), 0, (float)m_z - 1));
        
        int ixMax = static_cast<int>(Clamp((obj_box.m_maxX - min.m_x) * (float)m_x / (max.m_x - min.m_x), 0, (float)m_x - 1));
        int iyMax = static_cast<int>(Clamp((obj_box.m_maxY - min.m_y) * (float)m_y / (max.m_y - min.m_y), 0, (float)m_y - 1));
        int izMax = static_cast<int>(Clamp((obj_box.m_maxZ - min.m_z) * (float)m_z / (max.m_z - min.m_z), 0, (float)m_z - 1));

        //now add this object to all the cells between i Min and i Max
        for(int iZ = izMin; iZ <= izMax; ++iZ) //for every z cell
            for(int iY = iyMin; iY <= iyMax; ++iY) //for every y cell
                for(int iX = ixMin; iX <= ixMax; ++iX) //for every x cell
                {
                    //calculate the current index
                    index = iX + m_x * iY + m_x * m_y * iZ;
                    //if the number of stuff in this cell is currently 0,
                    if(counts[index] == 0)
                    {
                        m_cells[index] = new CellCache(objects[i]);
                        counts[index]++;
                    }
                    else
                    {
                        //make the last item in the list point to the new object in this cell
                        CellCache* next = new CellCache(objects[i]);
                        CellCache* current = m_cells[index];
                        while(current->m_next != NULL)
                            current = current->m_next;
                        current->m_next = next;
                        counts[index]++;
                    }
                }
    }
    //done?!
}
void Grid::CleanUp()
{
    //the only thigns we NEW are cell caches in m_cells;
    for(unsigned int i = 0; i < m_cells.size(); ++i)
    {
        CellCache* temp = m_cells[i];
        while(temp)
        {
            CellCache* store = temp->m_next;
            delete temp;
            temp = store;
        }
    }
}
bool Grid::Hit( const Ray& ray, float& tmin, ContactData& data ) const
{
    //usefull to store day data up here,
    float ox = ray.m_origin.m_x;
    float oy = ray.m_origin.m_y;
    float oz = ray.m_origin.m_z;
    float dx = ray.m_dir.m_x;
    float dy = ray.m_dir.m_y;
    float dz = ray.m_dir.m_z;
    //also the box data
    float x0 = m_box.m_minX;
    float y0 = m_box.m_minY;
    float z0 = m_box.m_minZ;
    float x1 = m_box.m_maxX;
    float y1 = m_box.m_maxY;
    float z1 = m_box.m_maxZ;

    // the following code includes modifications
    // from 'Realistic Ray Tracing' Shirley, P. Morley,R,K (2003),
    float tx_max, ty_max, tz_max; 
    float tx_min, ty_min, tz_min;
    float t0, t1;
    //a is one over the x direction of the ray...What to do if it is 0 ?
    // (float a = 1.0 / dx...this may fail if dx == 0)
    // This decides which direction to step the ray in...
    // so if a component is 0, then it is parallel to an axis of the grid
    // hmm...
    {
        /************************************************************************/
        /* Standard Box intersection Code Pasted here: Need the values stored   */
        /************************************************************************/
        float a = 1.0f / (dx != 0 ? dx : 1.0f);
        if (a >= 0) //if x is in a positive direction
        {
            tx_min = (x0 - ox) * a;
            tx_max = (x1 - ox) * a;
        }
        else //x is negative...
        {
            tx_min = (x1 - ox) * a;
            tx_max = (x0 - ox) * a;
        }
        //same for y
        float b = 1.0f / (dy != 0 ? dy : 1.0f);
        if (b >= 0) 
        {
            ty_min = (y0 - oy) * b;
            ty_max = (y1 - oy) * b;
        }
        else 
        {
            ty_min = (y1 - oy) * b;
            ty_max = (y0 - oy) * b;
        }
        //same for z
        float c = 1.0f / (dz != 0 ? dz : 1.0f);
        if (c >= 0) {
            tz_min = (z0 - oz) * c;
            tz_max = (z1 - oz) * c;
        }
        else 
        {
            tz_min = (z1 - oz) * c;
            tz_max = (z0 - oz) * c;
        }

        if (tx_min > ty_min)
            t0 = tx_min;
        else
            t0 = ty_min;

        if (tz_min > t0)
            t0 = tz_min;

        if (tx_max < ty_max)
            t1 = tx_max;
        else
            t1 = ty_max;

        if (tz_max < t1)
            t1 = tz_max;

        //Does not hit the grid at all ?
        if (t0 > t1)
            return(false);
    }

    //starting cell co-ordinates
    int ix, iy, iz;
    if(m_box.IsVectorInside(ray.m_origin))
    {
        ix = static_cast<int>(Clamp((ox - x0) * (float)m_x / (x1 - x0), 0 , (float)m_x -1));
        iy = static_cast<int>(Clamp((oy - y0) * (float)m_y / (y1 - y0), 0 , (float)m_y -1));
        iz = static_cast<int>(Clamp((oz - z0) * (float)m_z / (z1 - z0), 0 , (float)m_z -1));
    }
    else
    {
        Vec3 p = ray.m_origin + (ray.m_dir * t0);
        ix = static_cast<int>(Clamp((p.m_x - x0) * (float)m_x / (x1 - x0), 0 , (float)m_x -1));
        iy = static_cast<int>(Clamp((p.m_y - y0) * (float)m_y / (y1 - y0), 0 , (float)m_y -1));
        iz = static_cast<int>(Clamp((p.m_z - z0) * (float)m_z / (z1 - z0), 0 , (float)m_z -1));
    }

    //Now we compute the step values for X,Y,Z axis
    //Altho the intersections with the grid are unequally spaces along the ray itself
    //the X, Y, and Z axis co-ordinates of the intersections are equal! AMAZING
    //so we can work them out, and just step along the grid (like rasterization) 

    float dtx = (tx_max - tx_min) / m_x;
    float dty = (ty_max - ty_min) / m_y;
    float dtz = (tz_max - tz_min) / m_z;

    float 	tx_next, ty_next, tz_next;
    int 	ix_step, iy_step, iz_step;
    int 	ix_stop, iy_stop, iz_stop;
    //if direction in X is > 0...
    if (dx > 0) 
    {
        tx_next = tx_min + (ix + 1) * dtx;
        ix_step = +1;
        ix_stop = m_x;
    }
    else 
    {
        tx_next = tx_min + (m_x - ix) * dtx;
        ix_step = -1;
        ix_stop = -1;
    }

    if (dx == 0.0) 
    {
        tx_next = FLT_MAX;
        ix_step = -1;
        ix_stop = -1;
    }

    //Same for y
    if (dy > 0) 
    {
        ty_next = ty_min + (iy + 1) * dty;
        iy_step = +1;
        iy_stop = m_y;
    }
    else {
        ty_next = ty_min + (m_y - iy) * dty;
        iy_step = -1;
        iy_stop = -1;
    }

    if (dy == 0.0) 
    {
        ty_next = FLT_MAX;
        iy_step = -1;
        iy_stop = -1;
    }

    if (dz > 0) 
    {
        tz_next = tz_min + (iz + 1) * dtz;
        iz_step = +1;
        iz_stop = m_z;
    }
    else 
    {
        tz_next = tz_min + (m_z - iz) * dtz;
        iz_step = -1;
        iz_stop = -1;
    }

    if (dz == 0.0) 
    {
        tz_next = FLT_MAX;
        iz_step = -1;
        iz_stop = -1;
    }
    float far = m_scene.m_camera->GetFar();
    Vec3 endPosition = ray.m_origin + (ray.m_dir * far);
    far *= far; //sqr it
    if(m_box.IsVectorInside(endPosition))
    {
        float nX,nY,nZ;
        nX = endPosition.m_x; nY = endPosition.m_y; nZ = endPosition.m_z;
        ix_stop = static_cast<int>(Clamp((nX - x0) * (float)m_x / (x1 - x0), 0 , (float)m_x -1)) + ix_step;
        iy_stop = static_cast<int>(Clamp((nY - y0) * (float)m_y / (y1 - y0), 0 , (float)m_y -1)) + iy_step;
        iz_stop = static_cast<int>(Clamp((nZ - z0) * (float)m_z / (z1 - z0), 0 , (float)m_z -1)) + iz_step;
    }
    else
    {
        //the cameras far clip is outside of the grid,
        //we don't need to add an early out
    }
    //We have everything we need to do the traversal now, so while(true)...until we return a boolean
    while(true)
    {
        int index = ix + m_x * iy + m_x * m_y * iz;
        CellCache* cell = m_cells[index];
        ContactData temp(&m_scene);
        float		t = FLT_MAX;	
        while(cell)
        {
            Renderable* object = cell->m_object;		
            if ((ray.m_hitObject != object) && //if this ray has not already been checked against this
                object->Hit(ray, t, temp) && (t < tmin)) 
            {
                data.m_hitTarget = temp.m_hitTarget;
                data.m_hitNormal = temp.m_hitNormal;
                data.m_hitPoint = temp.m_hitPoint;
                data.m_hit	        = true;
                if(temp.HasColorStorred())
                    data.SetColor(temp.GetColor());
                tmin 				= t; 
            }
            cell = cell->m_next;
        }
        if(data.m_hit == true)
        {   
            return true;
        }
        //now manage advancing the ray
        if (tx_next < ty_next && tx_next < tz_next) 
        {
            tx_next += dtx;
            ix += ix_step;
            if (ix == ix_stop)
                return false;
        } 
        else 
        { 	
            if (ty_next < tz_next) 
            {
                ty_next += dty;
                iy += iy_step;
                if (iy == iy_stop)
                    return (false);
            }
            else 
            {		
                tz_next += dtz;
                iz += iz_step;
                if (iz == iz_stop)
                    return (false);
            }
        }
        //check if we have gone too far
        
    }
    return false;//never hit!
}
const BBox& Grid::GetBox() const
{
    return m_box;
}


#ifdef DEBUGGING
bool Grid::Hit_Debug( const Ray& ray, float& tmin, ContactData& data ) const
{
    //usefull to store day data up here,
    float ox = ray.m_origin.m_x;
    float oy = ray.m_origin.m_y;
    float oz = ray.m_origin.m_z;
    float dx = ray.m_dir.m_x;
    float dy = ray.m_dir.m_y;
    float dz = ray.m_dir.m_z;
    //also the box data
    float x0 = m_box.m_minX;
    float y0 = m_box.m_minY;
    float z0 = m_box.m_minZ;
    float x1 = m_box.m_maxX;
    float y1 = m_box.m_maxY;
    float z1 = m_box.m_maxZ;

    // the following code includes modifications
    // from 'Realistic Ray Tracing' Shirley, P. Morley,R,K (2003),
    float tx_max, ty_max, tz_max; 
    float tx_min, ty_min, tz_min;
    float t0, t1;
    //a is one over the x direction of the ray...What to do if it is 0 ?
    // (float a = 1.0 / dx...this may fail if dx == 0)
    // This decides which direction to step the ray in...
    // so if a component is 0, then it is parallel to an axis of the grid
    // hmm...
    {
        /************************************************************************/
        /* Standard Box intersection Code Pasted here: Need the values stored   */
        /************************************************************************/
        float a = 1.0f / (dx != 0 ? dx : 1.0f);
        if (a >= 0) //if x is in a positive direction
        {
            tx_min = (x0 - ox) * a;
            tx_max = (x1 - ox) * a;
        }
        else //x is negative...
        {
            tx_min = (x1 - ox) * a;
            tx_max = (x0 - ox) * a;
        }
        //same for y
        float b = 1.0f / (dy != 0 ? dy : 1.0f);
        if (b >= 0) 
        {
            ty_min = (y0 - oy) * b;
            ty_max = (y1 - oy) * b;
        }
        else 
        {
            ty_min = (y1 - oy) * b;
            ty_max = (y0 - oy) * b;
        }
        //same for z
        float c = 1.0f / (dz != 0 ? dz : 1.0f);
        if (c >= 0) {
            tz_min = (z0 - oz) * c;
            tz_max = (z1 - oz) * c;
        }
        else 
        {
            tz_min = (z1 - oz) * c;
            tz_max = (z0 - oz) * c;
        }

        if (tx_min > ty_min)
            t0 = tx_min;
        else
            t0 = ty_min;

        if (tz_min > t0)
            t0 = tz_min;

        if (tx_max < ty_max)
            t1 = tx_max;
        else
            t1 = ty_max;

        if (tz_max < t1)
            t1 = tz_max;

        //Does not hit the grid at all ?
        if (t0 > t1)
        {
            return(false);
        }
    }

    //starting cell co-ordinates
    int ix, iy, iz;
    if(m_box.IsVectorInside(ray.m_origin))
    {
        ix = static_cast<int>(Clamp((ox - x0) * (float)m_x / (x1 - x0), 0 , (float)m_x -1));
        iy = static_cast<int>(Clamp((oy - y0) * (float)m_y / (y1 - y0), 0 , (float)m_y -1));
        iz = static_cast<int>(Clamp((oz - z0) * (float)m_z / (z1 - z0), 0 , (float)m_z -1));
    }
    else
    {
        Vec3 p = ray.m_origin + (ray.m_dir * t0);
        ix = static_cast<int>(Clamp((p.m_x - x0) * (float)m_x / (x1 - x0), 0 , (float)m_x -1));
        iy = static_cast<int>(Clamp((p.m_y - y0) * (float)m_y / (y1 - y0), 0 , (float)m_y -1));
        iz = static_cast<int>(Clamp((p.m_z - z0) * (float)m_z / (z1 - z0), 0 , (float)m_z -1));
    }

    //Now we compute the step values for X,Y,Z axis
    //Altho the intersections with the grid are unequally spaces along the ray itself
    //the X, Y, and Z axis co-ordinates of the intersections are equal! AMAZING
    //so we can work them out, and just step along the grid (like rasterization) 

    float dtx = (tx_max - tx_min) / m_x;
    float dty = (ty_max - ty_min) / m_y;
    float dtz = (tz_max - tz_min) / m_z;

    float 	tx_next, ty_next, tz_next;
    int 	ix_step, iy_step, iz_step;
    int 	ix_stop, iy_stop, iz_stop;
    //if direction in X is > 0...
    if (dx > 0) 
    {
        tx_next = tx_min + (ix + 1) * dtx;
        ix_step = +1;
        ix_stop = m_x;
    }
    else 
    {
        tx_next = tx_min + (m_x - ix) * dtx;
        ix_step = -1;
        ix_stop = -1;
    }

    if (dx == 0.0) 
    {
        tx_next = FLT_MAX;
        ix_step = -1;
        ix_stop = -1;
    }

    //Same for y
    if (dy > 0) 
    {
        ty_next = ty_min + (iy + 1) * dty;
        iy_step = +1;
        iy_stop = m_y;
    }
    else {
        ty_next = ty_min + (m_y - iy) * dty;
        iy_step = -1;
        iy_stop = -1;
    }

    if (dy == 0.0) 
    {
        ty_next = FLT_MAX;
        iy_step = -1;
        iy_stop = -1;
    }

    if (dz > 0) 
    {
        tz_next = tz_min + (iz + 1) * dtz;
        iz_step = +1;
        iz_stop = m_z;
    }
    else 
    {
        tz_next = tz_min + (m_z - iz) * dtz;
        iz_step = -1;
        iz_stop = -1;
    }

    if (dz == 0.0) 
    {
        tz_next = FLT_MAX;
        iz_step = -1;
        iz_stop = -1;
    }

    //We have everything we need to do the traversal now, so while(true)...until we return a boolean
    while(true)
    {
        int index = ix + m_x * iy + m_x * m_y * iz;
        CellCache* cell = m_cells[index];
        ContactData temp(&m_scene);
        float		t = FLT_MAX;	
        while(cell)
        {
            Renderable* object = cell->m_object;	
            if ((ray.m_hitObject != object) && //if this ray has not already been checked against this
                object->Hit_Debug(ray, t, temp) && (t < tmin)) 
            {
                OUT_STRING("Ray Hit! : ");
                OUT_STRING(object->m_information);
                data.m_hitTarget = temp.m_hitTarget;
                data.m_hitNormal = temp.m_hitNormal;
                data.m_hitPoint = temp.m_hitPoint;
                data.m_hit	        = true;
                if(temp.HasColorStorred())
                    data.SetColor(temp.GetColor());
                tmin 				= t; 
            }
            cell = cell->m_next;
        }
        if(data.m_hit == true)
        {   
            return true;
        }
        //now manage advancing the ray
        if (tx_next < ty_next && tx_next < tz_next) 
        {
            tx_next += dtx;
            ix += ix_step;
            if (ix == ix_stop)
            {
                return false;
            }
        } 
        else 
        { 	
            if (ty_next < tz_next) 
            {
                ty_next += dty;
                iy += iy_step;
                if (iy == iy_stop)
                {
                    return (false);
                }
            }
            else 
            {		
                tz_next += dtz;
                iz += iz_step;
                if (iz == iz_stop)
                {
                    return (false);
                }
            }
        }
    }
    OUT_STRING("Ray Missed");
    return false;//never hit!
}
#endif

